

import lejos.nxt.*;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.*;


public class ForcesCar {

	static double SIDE_SIZE = 40.0;
	static double WHEEL_DIAMETER = 5.6f;
	static double AXIS_LENGTH = 5.6f;
	
	public static void main (String[] args) throws Exception {
		
		// Navigator:
		DifferentialPilot navigator = new DifferentialPilot(WHEEL_DIAMETER, AXIS_LENGTH, Motor.A, Motor.C);
		navigator.setAcceleration(2000);
		
		// Instantiating Sensor ports:
		LightSensor lightLeft  = new LightSensor(SensorPort.S2);
		LightSensor lightRight = new LightSensor(SensorPort.S1);
		
		UltrasonicSensor ultrasonic = new UltrasonicSensor(SensorPort.S3);
		
		// Calibrate sensors if needed:
		lightLeft.setFloodlight(false);
		lightRight.setFloodlight(false);
		
		// Set Behaviours
		Behavior b1 = new DriveForwardAndLook(navigator);
		Behavior b3 = new VirtualForces(navigator, ultrasonic, lightLeft, lightRight);
		Behavior [] bArray = {b1, b3};
		
		Arbitrator arby = new Arbitrator(bArray);
		arby.start();
	}
}
